package butines.game2.graph;

import java.util.ArrayList;
import java.util.Arrays;
import java.util.Collections;
import java.util.Comparator;
import java.util.HashSet;
import java.util.LinkedList;
import java.util.List;
import java.util.Set;

public class SearchAStar {

    private SparseGraph<NavGraphNode, NavGraphEdge> graph;

    private float[] gCosts;
    private float[] fCosts;
    private int[] navigated;
    
    private int attempts = 32;
    
    private final Comparator<Integer> order = new Comparator<Integer>() {
        @Override
        public int compare(Integer a, Integer b) {
            if (fCosts[a] < fCosts[b]) {
                return -1;
            } else if (fCosts[a] > fCosts[b]) {
                return 1;
            }
            return 0;
        }
    };
    
    public float distance(int from, int to) {
        NavGraphNode a = graph.getNode(from);
        NavGraphNode b = graph.getNode(to);
        return a.getPosition().distance(b.getPosition());
    }
    
    public SearchAStar(SparseGraph<NavGraphNode, NavGraphEdge> graph) {
        this.graph = graph;
        gCosts = new float[graph.getNumNodes()];
        fCosts = new float[graph.getNumNodes()];
        navigated = new int[graph.getNumNodes()];
    }

    private List<Integer> reconstructPath(int currentNode) {
        LinkedList<Integer> path = new LinkedList<>();
        
        path.addFirst(currentNode);
        
        currentNode = navigated[currentNode];
        while (currentNode != -1) {
            path.addFirst(currentNode);
            currentNode = navigated[currentNode];
        }
        
        return path;
    }
    
    public List<Integer> search(int start, int goal) {
        
        Set<Integer> closedset = new HashSet<>();
        List<Integer> openset = new ArrayList<>();

        Arrays.fill(gCosts, 0);
        Arrays.fill(fCosts, 0);
        Arrays.fill(navigated, -1);
        
        gCosts[start] = 0;
        fCosts[start] = gCosts[start] + distance(start, goal);

        openset.add(start);
        
        while (!openset.isEmpty() && attempts-- > 0) {
        	
        	Collections.sort(openset, order);
            
            int current = openset.remove(0);
            
            if (current == goal) {
                return reconstructPath(goal);
            }
            
            closedset.add(current);
            
            for (NavGraphEdge edge : graph.getEdges(current)) {
                
                int neighbor = edge.getTo();
                if (closedset.contains(neighbor)) {
                    continue;
                }
            
                float gCost = gCosts[current] + distance(current, neighbor);
                
                if (!openset.contains(neighbor) || gCost < gCosts[neighbor]) {
                    navigated[neighbor] = current;
                    gCosts[neighbor] = gCost;
                    fCosts[neighbor] = gCosts[neighbor] + distance(neighbor, goal);
                    if (!openset.contains(neighbor)) {
                        openset.add(neighbor);
                    }
                }
                
            }
            
        }
        
        return Collections.emptyList();
        
    }
    
    
}
